// Devin Koepl

#include <gui/fca_gui.h>

int main(int argc, char **argv)
{
	ros::init(argc, argv, "fca_gui");
	ros::NodeHandle n;
	fca_client = n.serviceClient<fca_controllers::fca_srv>("/gui_interface_srv");
	fca_srv.request.command = CMD_DISABLE;
	fca_srv.request.controller_requested = 0; // 0 for no controller, controllers need to be their own package I suppose.  Actually it would be better to move the gui inside the fca package.

	Gtk::Main gtk(argc, argv);

	// Create the relative path to the Glade file.
	std::string glade_gui_path = std::string(argv[0]);

	glade_gui_path = glade_gui_path.substr(0, glade_gui_path.rfind("/bin"));
	glade_gui_path = glade_gui_path.append("/src/fca_gui.glade");

	Glib::RefPtr<Gtk::Builder> gui = Gtk::Builder::create();
	try
	{
		gui->add_from_file(glade_gui_path);
	}
	catch(const Glib::FileError& ex)
	{
		ROS_ERROR("File Error");
//			ROS_ERROR("FileError: %d", ex.what());
	}
	catch(const Gtk::BuilderError& ex)
	{
		ROS_ERROR("Builder Error");
//			ROS_ERROR("BuilderError: %d", ex.what());
	}

	// Grab pointers to GUI objects
	gui->get_widget("fca_window", window);
	if(!window)
	{
		ROS_ERROR("No FCA Window");
	}

	gui->get_widget("controller_notebook", controller_notebook);

	gui->get_widget("motor_torque_hscale", motor_torque_hscale);

	gui->get_widget("motor_position_hscale", motor_position_hscale);
	gui->get_widget("p_motor_position_hscale", p_motor_position_hscale);
	gui->get_widget("d_motor_position_hscale", d_motor_position_hscale);


	gui->get_widget("drawing_area", drawing_area);

	gui->get_widget("motor_torque_progress_bar", motor_torque_progress_bar);

	gui->get_widget("log_file_chkbox", log_file_chkbox);
	gui->get_widget("log_file_chooser", log_file_chooser);

	gui->get_widget("restart_button", restart_button);
	gui->get_widget("enable_button", enable_button);
	gui->get_widget("disable_button", disable_button);

	gui->get_widget("log_start_toggle", log_start_toggle);

	// Initialize GUI objects
	motor_torque_hscale->set_range(MTR_MIN_TRQ, MTR_MAX_TRQ);

	motor_position_hscale->set_range(2.0, 3.5);
	p_motor_position_hscale->set_range(0., 1000.);
	d_motor_position_hscale->set_range(0., 50.);

	motor_torque_progress_bar->set_fraction(0.);

	// Create the path to the data file with the last state of the gui gains.
	std::string gui_state_file = std::string(argv[0]);
	gui_state_file = gui_state_file.substr(0, gui_state_file.rfind("/bin"));
	gui_state_file = gui_state_file.append("/src/gui_last_state.dat");



	drawing_allocation = drawing_area->get_allocation();

	// Connect buttons to functions.
	log_file_chkbox->signal_toggled().connect( sigc::ptr_fun(log_chkbox_toggled) );
	
	log_start_toggle->signal_toggled().connect( sigc::ptr_fun(log_chkbox_toggled) );
	
	restart_button->signal_clicked().connect( sigc::ptr_fun( restart_robot ) );
	enable_button->signal_clicked().connect( sigc::ptr_fun( enable_motors ) );
	disable_button->signal_clicked().connect( sigc::ptr_fun( disable_motors ) );

	controller_notebook->signal_switch_page().connect( sigc::ptr_fun(switch_controllers) );

	sigc::connection conn = Glib::signal_timeout().connect(sigc::ptr_fun(poke_controller), 100); // 50 is the timeout in milliseconds

	gtk.run(*window);

	// Store the final state of the gui.
	/*gui_state_fp = fopen(gui_state_file.c_str(), "w");

	fprintf(gui_state_fp, "Motor Position P Gain: %f\n", p_motor_position_hscale->get_value());
	fprintf(gui_state_fp, "Motor Position D Gain: %f\n", d_motor_position_hscale->get_value());
	fprintf(gui_state_fp, "Leg Position P Gain: %f\n", p_leg_position_hscale->get_value());
	fprintf(gui_state_fp, "Leg Position D Gain: %f\n", d_leg_position_hscale->get_value());
	fprintf(gui_state_fp, "Sine Wave P Gain: %f\n", p_sine_wave_hscale->get_value());
	fprintf(gui_state_fp, "Sine Wave D Gain: %f\n", d_sine_wave_hscale->get_value());
	fprintf(gui_state_fp, "Raibert Velocity Gain: %f\n", raibert_hor_vel_gain_hscale->get_value());
	fprintf(gui_state_fp, "Raibert Height Gain: %f\n", raibert_leg_force_gain_hscale->get_value());
	fprintf(gui_state_fp, "Raibert Leg TD Angle Gain: %f\n", raibert_leg_angle_gain_hscale->get_value());
	fprintf(gui_state_fp, "Raibert P Gain: %f\n", raibert_stance_p_gain_hscale->get_value());
	fprintf(gui_state_fp, "Raibert D Gain: %f\n", raibert_stance_d_gain_hscale->get_value());
	fprintf(gui_state_fp, "Raibert Spring Deflection Threshold: %f\n", raibert_stance_spring_threshold_hscale->get_value());

	fclose(gui_state_fp);*/

  return 0;
}

void log_chkbox_toggled( void )
{
	if ( log_file_chkbox->get_active() )
	{
		// Open the log file for data logging.
		log_file_fp = fopen(log_file_chooser->get_filename().c_str(), "w");
		fprintf(log_file_fp, "time, mtr_ang, leg_ang, mtr_trq\n");
	}
	else
	{
		// Close the log file.
		fclose(log_file_fp);
	}
}

void restart_robot( void )
{
	if ( fca_srv.request.command == CMD_DISABLE )
	{
		fca_srv.request.command = CMD_RESTART;
		fca_srv.request.controller_requested = NO_CONTROLLER;
		controller_notebook->set_current_page( NO_CONTROLLER );
	}
}

void enable_motors( void )
{
	fca_srv.request.command = CMD_RUN;
}

void disable_motors( void )
{
	fca_srv.request.command = CMD_DISABLE;
}

void log_start( void )
{
	fca_srv.request.command = CMD_DISABLE;
}

void switch_controllers(GtkNotebookPage* page, guint page_num)
{
	if ( fca_srv.request.command == CMD_DISABLE )
		fca_srv.request.controller_requested = page_num;
	else
		controller_notebook->set_current_page( fca_srv.request.controller_requested );
}

bool poke_controller( void )
{
	char buffer[20];

	switch (fca_srv.request.controller_requested)
	{
		case NO_CONTROLLER:
			//MTR_TRQ_CONTROLLER_DATA(&(fca_srv.request.control_data.elems))->mtr_trqA = 0.;
			break;
		case MOTOR_TORQUE_CONTROLLER:
			((MtrTrqControllerData *)(&(fca_srv.request.control_data.elems)))->mtr_trq = motor_torque_hscale->get_value();
			break;
		case MOTOR_POSITION_CONTROLLER:
			((MtrPosControllerData *)(&(fca_srv.request.control_data.elems)))->mtr_ang = motor_position_hscale->get_value();
			((MtrPosControllerData *)(&(fca_srv.request.control_data.elems)))->p_gain = p_motor_position_hscale->get_value();
			((MtrPosControllerData *)(&(fca_srv.request.control_data.elems)))->d_gain = d_motor_position_hscale->get_value();
			break;

	}

	// Check to see if we are supposed to be logging data.
	if ( log_file_chkbox->get_active() )
		fprintf(log_file_fp, "%f, %f, %f, %f\n",
				fca_srv.response.time,
				fca_srv.response.motor_angle,
				fca_srv.response.leg_angle,
				fca_srv.response.motor_torque);
				

	if ( fca_client.call( fca_srv ) )
		draw_leg();

	if ( fca_srv.request.command == CMD_RESTART )
		fca_srv.request.command = CMD_DISABLE;

	// Move the sliders if the controller is disabled.
	if ( fca_srv.response.status == CMD_DISABLE )
	{
		motor_position_hscale->set_value(fca_srv.response.motor_angle);
	}

	// Update the motor torque progress bars.
	motor_torque_progress_bar->set_fraction( MIN ( ABS(fca_srv.response.motor_torque), MTR_MAX_TRQ ) / MTR_MAX_TRQ);


	return true;
}

void draw_leg()
{
	float segment_length = 125.;
	float short_segment_length = 100.;
	float motor_radius = 70.;

	float start_x = 260.;
	float start_y = 100.;

	drawing_area->get_window()->clear();

	cr = drawing_area->get_window()->create_cairo_context();

	cr->set_line_width(12.0);

	// Draw the motors
	cr->set_source_rgb(0.0, 0.8, 0.0);
	// A
	cr->move_to(start_x, start_y);
	cr->rel_line_to(motor_radius * cos(fca_srv.response.motor_angle), -motor_radius * sin(fca_srv.response.motor_angle));
	cr->move_to(start_x, start_y);
	cr->rel_line_to(-motor_radius * cos(fca_srv.response.motor_angle), motor_radius * sin(fca_srv.response.motor_angle));

	cr->stroke();

	// Draw the leg
	cr->move_to(start_x, start_y);
	// OSU orange 216, 90, 26
	cr->set_source_rgb(0.8471, 0.3529, 0.1020);
	//cr->set_source_rgb(0.8, 0.0, 0.0);
	// A
	cr->rel_line_to(short_segment_length * cos(fca_srv.response.leg_angle), -short_segment_length * sin(fca_srv.response.leg_angle));

	cr->stroke();
}
